/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SpeedController;

/**
 *
 * @author Administrator
 */
public class Team997CubicDrive extends RobotDrive {
    private boolean m_bReverseJoy = false;

    public Team997CubicDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
            SpeedController frontRightMotor, SpeedController rearRightMotor) {
        super(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
    }
    
    public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
        if (leftStick == null || rightStick == null) {
            throw new NullPointerException("Null HID provided");
        }
        if (!m_bReverseJoy)
            tankDrive(leftStick.getY(), rightStick.getY());
        else
            tankDrive(-rightStick.getY(), -leftStick.getY());
    }

    //This year's robot is better as square drive.
//    public void tankDrive(double leftValue, double rightValue) {
//        leftValue = limit(leftValue);
//        rightValue = limit(rightValue);
//        leftValue = leftValue * leftValue * leftValue;
//        rightValue = rightValue * rightValue * rightValue;
//        setLeftRightMotorOutputs(leftValue, rightValue);
//    }

    public void arcadeDrive(double moveValue, double rotateValue, boolean cubedInputs) {
        // local variables to hold the computed PWM values for the motors
        double leftMotorSpeed;
        double rightMotorSpeed;

        moveValue = limit(moveValue);
        rotateValue = limit(rotateValue);

        if (cubedInputs) {
            // square the inputs (while preserving the sign) to increase fine control while permitting full power
            moveValue = (moveValue * moveValue * moveValue);
            rotateValue = (rotateValue * rotateValue * rotateValue);
        }

        if (moveValue > 0.0) {
            if (rotateValue > 0.0) {
                leftMotorSpeed = moveValue - rotateValue;
                rightMotorSpeed = Math.max(moveValue, rotateValue);
            } else {
                leftMotorSpeed = Math.max(moveValue, -rotateValue);
                rightMotorSpeed = moveValue + rotateValue;
            }
        } else {
            if (rotateValue > 0.0) {
                leftMotorSpeed = -Math.max(-moveValue, rotateValue);
                rightMotorSpeed = moveValue + rotateValue;
            } else {
                leftMotorSpeed = moveValue - rotateValue;
                rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
            }
        }

        setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
    }

    public void setReverse(boolean bReverseJoy) {
        m_bReverseJoy = bReverseJoy;
    }
}
